
#include "sample_data.hpp"
#include "build_data.hpp"
// 目标 使用ransac算法对直线进行拟合

using namespace rc;



int main(int argc, char** argv){

    // 1、生成数据
    build_line_data<float> bd; 
    
    std::vector<Eigen::Matrix<float, 3, 1>> outXs;
    std::vector<Eigen::Matrix<float, 3, 1>> outYs;

    bd.buid(outXs, outYs,150, 100); // 构建100 个内点
    // 2、采样数据
    // A 9 + b 3 共要 12 个参数  形成的H矩阵是12 × 12 的， 每一组能有3个约束，所以需要四对点才能得到解
    sample_data sample(300, 4, outXs.size()); // 采样300次， 一次采集4个 采集数据集的大小
    std::vector<std::vector<int>> dataId;
    sample(dataId);
    // 3、采样数据进行拟合计算最好组别
    std::vector<std::vector<double>> outXsVec(outXs.size());
    std::vector<std::vector<double>> outYsVec(outXs.size());
    for(size_t i = 0; i < outXs.size(); i++){
        outXsVec[i].push_back(outXs[i](0, 0));
        outXsVec[i].push_back(outXs[i](1, 0));
        outXsVec[i].push_back(outXs[i](2, 0));
        outYsVec[i].push_back(outYs[i](0, 0));
        outYsVec[i].push_back(outYs[i](1, 0));
        outYsVec[i].push_back(outYs[i](2, 0));
    }
    modelType* md = new lineType;
    for(size_t i = 0; i < dataId.size(); ++i){
        sample.solveAndEvaluation(md, dataId[i], outXs, outYs);
        md->evaluate(outXsVec, outYsVec, 0.5);
    }
    std::cout << "best A " << std::endl << dynamic_cast<lineType*>(md)->mBestA << std::endl;
    std::cout << "best B " << std::endl << dynamic_cast<lineType*>(md)->mBestB << std::endl;
    
    return 0;

}




